#pragma once
#include <string>
#include <vector>
#include <memory>
#include <iostream>
#include <functional>

#include "rtdev/robot_status_common.h"

namespace rtdev {
class ServoItf;
}

namespace axis {
const size_t MAX_AXIS_CNT = 16;

class Axis;
class Planner;

class Axis
{
public:
    static const int kEnableTimeout = 2000;

public:
    enum ErrorCode
    {
        ERR_NONE = 0,
        ERR_OFFLINE = 10,
        ERR_SHUTDOWN,
        ERR_ENABLE_TIMEOUT,
        ERR_LOST_POWER,
        ERR_SF_CAT1_STOP,
    };
    enum class OpMode
    {
        OP_MODE_HOME = 6,
        OP_MODE_CSP = 8,
        OP_MODE_CST = 10
    };
    enum class RunState
    {
        Disable = 0,  // 未使能
        Standstill,   // 静止
        ErrorStop,
        Sync,  // 同步运动，与其他轴或轴组同步
    };
    struct Config
    {
        double cycletime_s;
        double servo_time_s;

        double max_pos;
        double min_pos;
        double max_vel;
        double max_acc;
        double max_jerk;
        double max_torq;

        double cat1_stop_dec;

        int dir;                   // 电机旋转方向
        double joint_zero_offset;  // DH 零位与伺服零位差值
        double gear_ratio;         // 减速比
        double motor_ratio_torq;   // 电机额定转矩
    };

public:
    //
    Axis(int id = 0);
    int init();
    void boot_init(std::string bus_name, int devid);  //上电初始化伺服
    void link_grp(int grp_id);                        //关联到轴组
    void set_sim(bool sim);
    void task_update(double ts);
    void reset_planner();  // 由于booting阻塞导致planner无数据，booting结束后需要清除该错误状态

public:
    virtual RunState run_state();
    void change_fsm_stat(RunState new_sta);
    void change_to_errstat(uint32_t ecode);

private:
    // fsm
    RunState run_state_;
    RunState next_run_state_;
    void fsm_stat_disable(bool is_first);
    void fsm_stat_standstill(bool is_first);
    void fsm_stat_errorstop(bool is_first);
    void fsm_stat_sync(bool is_first, double ts);

private:
    double get_pos_from_encode(int pos);
    double get_torq_from_encode(int torq);
    double get_pos_encode(double pos);
    double get_torq_encode(double tor);

public:
    Config config_;
    inline const Config& config() { return config_; }
    virtual int get_id();
    virtual bool is_torque_over();
    int get_linked_grp_id() const { return linked_grp_id_; }

public:
    // servo operation
    std::shared_ptr<rtdev::ServoItf> servo_;
    int block_get_sdo(uint32_t objid, void* data, int s, int retry);
    virtual int async_set_sdo(uint32_t obj_id, int* val, std::function<int(uint8_t*, int)> cb, int timeout_ms = 160, int retry = 3);
    virtual int async_get_sdo(uint32_t obj_id, int* val, std::function<int(uint8_t*, int)> cb, int timeout_ms = 160, int retry = 3);

public:
    void shutdown();      //关闭电源的时候调用
    int enable(bool en);  //使能控制
    void set_home();      // 触发回零
    void reset_error();
    void reset_collision();
    bool is_alive();
    bool is_homing();  //是否在回零
    bool is_homed();   // 回零是否完成

    void set_axis_tracking_mode(OpMode mode);  // 0 for CSP 位置跟踪； 1 for CST 转矩跟踪; 2 for Home 回零
    /**
     * @brief 接受axisgroup规划的粗插补位置，会在axis内部进行精插补
     * @param pos: 目标位置
     * @param torq_off: 前馈力矩补偿
     * @return 0: 成功 -1: 失败
    */
    int sync_motion_plan(double pos, double torque, bool is_jump);
    /**
     * @brief rtbus中的叠加数据，为最终的伺服指令
     * @param pos: 目标位置
     * @param torque: 前馈力矩补偿
     * @return 0: 成功 -1: 失败
    */
    int sync_servo_move(double pos, double torque);
    void sync_collision_lv(uint8_t lv);
    void set_torque_forward(bool enable);
    void set_traj_time(double servo_s, double motion_s);

    // set interface
    void set_max_pos(double pos) { this->config_.max_pos = pos; }
    void set_min_pos(double pos) { this->config_.min_pos = pos; }
    void set_max_vel(double vel) { this->config_.max_vel = vel; }
    void set_max_acc(double acc) { this->config_.max_acc = acc; }
    void set_max_jerk(double jerk) { this->config_.max_jerk = jerk; }

private:
    int axis_id_;
    int linked_grp_id_;
    // disable stat
    int enable_timeout_;
    // error stat
    uint32_t error_code_;

public:
    struct UsrCmd
    {
        bool enable;  //使能控制
        uint8_t collision_reset;
        bool error_reset;
        uint8_t home;  //0-无回零 1-回零指令
        uint8_t opmode;
        bool enable_dyn_forward;
        uint8_t collision_lv_;  //碰撞检测等级给定
        uint16_t control_word_;
    } cmd_;
    JointPeriodStatus status_;
    void update_joint_fdb_status(JointPeriodStatus s);  // 每个周期由axis_robot负责刷新
    JointPeriodStatus get_status() const { return status_; }

public:
    void rt_task_update();
    void update_servo_cmd(double ts, double& pos);

    //插补
private:
    void do_interp();                       //在rtdev周期更新的中，倍频插值时调用
    std::shared_ptr<Planner> pos_planner_;  // 位置插补器
    std::shared_ptr<Planner> tor_planner_;  // 力矩插补器
    struct RefTarget
    {
        double pos = 0;
        double vel = 0;  //位置模式下，pos的差分值
        double acc = 0;  //位置模式下，vel的差分值
        double jerk = 0;
    };
    RefTarget target_, prev_target_;  //从上层粗插补获取到的参考目标

    int do_plan(double s0, double se, double v0, double ve, double a0, double ae, double dt);
};

class AxisMgr
{
public:
    static AxisMgr& get();
    std::shared_ptr<Axis> axis(uint8_t axis_id);
    inline size_t count() const { return axis_.size(); }

private:
    AxisMgr();
    AxisMgr(const AxisMgr&) = delete;
    AxisMgr& operator=(const AxisMgr&) = delete;

private:
    std::vector<std::shared_ptr<Axis>> axis_;
};
}  // namespace axis
